Stablize the schur ordering algorithm.

The schur ordering is used to construct an elimination
ordering for Schur type solvers when the user has not
supplied an elimination ordering.

The ordering algorithm does an ordered traversal of the
sparsity graph of the Hessian. The order in which this is
done used to be determined by the degree of the parameter
blocks with ties broken arbitrarily using the memory address
of the parameter blocks.

This introduced non-determinism in the solver, causing subtle
numerical differences in the value of the solution everytime
the solve was run.

This change introduces ComputeStableSchurOrdering which utilizes
a new function StableIndependentSetOrdering. The latter takes
as input an ordering of the vertices of the graph which is used
to break ties when ordering the vertice by degree. The former
constructs such an ordering by using the order in which the
parameter blocks were added to the Problem.

In this way, as long as the construction of the problem is
deterministic, the schur ordering will always be deterministic
too.

I have chosen not to delete the existing unstable implementations
of these functions as they are used by the inner iteration
minimizer.

Sometime in the near future I will clean up some of the duplicate
code and see if we can move all the code to using a stable ordering.

Change-Id: I8fbfa240d7307a2c3fe9b135f6968aa410d78780
diff --git a/internal/ceres/graph_algorithms.h b/internal/ceres/graph_algorithms.h
index 2e6eec0..f38a13f 100644
--- a/internal/ceres/graph_algorithms.h
+++ b/internal/ceres/graph_algorithms.h
@@ -45,9 +45,9 @@
 
 // Compare two vertices of a graph by their degrees.
 template <typename Vertex>
-class VertexDegreeLessThan {
+class VertexTotalOrdering {
  public:
-  explicit VertexDegreeLessThan(const Graph<Vertex>& graph)
+  explicit VertexTotalOrdering(const Graph<Vertex>& graph)
       : graph_(graph) {}
 
   bool operator()(const Vertex& lhs, const Vertex& rhs) const {
@@ -61,6 +61,20 @@
   const Graph<Vertex>& graph_;
 };
 
+template <typename Vertex>
+class VertexDegreeLessThan {
+ public:
+  explicit VertexDegreeLessThan(const Graph<Vertex>& graph)
+      : graph_(graph) {}
+
+  bool operator()(const Vertex& lhs, const Vertex& rhs) const {
+    return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size();
+  }
+
+ private:
+  const Graph<Vertex>& graph_;
+};
+
 // Order the vertices of a graph using its (approximately) largest
 // independent set, where an independent set of a graph is a set of
 // vertices that have no edges connecting them. The maximum
@@ -104,7 +118,7 @@
 
 
   sort(vertex_queue.begin(), vertex_queue.end(),
-       VertexDegreeLessThan<Vertex>(graph));
+       VertexTotalOrdering<Vertex>(graph));
 
   // Iterate over vertex_queue. Pick the first white vertex, add it
   // to the independent set. Mark it black and its neighbors grey.
@@ -143,6 +157,81 @@
   return independent_set_size;
 }
 
+// Same as above with one important difference. The ordering parameter
+// is an input/output parameter which carries an initial ordering of
+// the vertices of the graph. The greedy independent set algorithm
+// starts by sorting the vertices in increasing order of their
+// degree. The input ordering is used to stabilize this sort, i.e., if
+// two vertices have the same degree then they are ordered in the same
+// order in which they occur in "ordering".
+//
+// This is useful in eliminating non-determinism from the Schur
+// ordering algorithm over all.
+template <typename Vertex>
+int StableIndependentSetOrdering(const Graph<Vertex>& graph,
+                                 vector<Vertex>* ordering) {
+  CHECK_NOTNULL(ordering);
+  const HashSet<Vertex>& vertices = graph.vertices();
+  const int num_vertices = vertices.size();
+  CHECK_EQ(vertices.size(), ordering->size());
+
+  // Colors for labeling the graph during the BFS.
+  const char kWhite = 0;
+  const char kGrey = 1;
+  const char kBlack = 2;
+
+  vector<Vertex> vertex_queue(*ordering);
+
+  stable_sort(vertex_queue.begin(), vertex_queue.end(),
+              VertexDegreeLessThan<Vertex>(graph));
+
+  // Mark all vertices white.
+  HashMap<Vertex, char> vertex_color;
+  for (typename HashSet<Vertex>::const_iterator it = vertices.begin();
+       it != vertices.end();
+       ++it) {
+    vertex_color[*it] = kWhite;
+  }
+
+  ordering->clear();
+  ordering->reserve(num_vertices);
+  // Iterate over vertex_queue. Pick the first white vertex, add it
+  // to the independent set. Mark it black and its neighbors grey.
+  for (int i = 0; i < vertex_queue.size(); ++i) {
+    const Vertex& vertex = vertex_queue[i];
+    if (vertex_color[vertex] != kWhite) {
+      continue;
+    }
+
+    ordering->push_back(vertex);
+    vertex_color[vertex] = kBlack;
+    const HashSet<Vertex>& neighbors = graph.Neighbors(vertex);
+    for (typename HashSet<Vertex>::const_iterator it = neighbors.begin();
+         it != neighbors.end();
+         ++it) {
+      vertex_color[*it] = kGrey;
+    }
+  }
+
+  int independent_set_size = ordering->size();
+
+  // Iterate over the vertices and add all the grey vertices to the
+  // ordering. At this stage there should only be black or grey
+  // vertices in the graph.
+  for (typename vector<Vertex>::const_iterator it = vertex_queue.begin();
+       it != vertex_queue.end();
+       ++it) {
+    const Vertex vertex = *it;
+    DCHECK(vertex_color[vertex] != kWhite);
+    if (vertex_color[vertex] != kBlack) {
+      ordering->push_back(vertex);
+    }
+  }
+
+  CHECK_EQ(ordering->size(), num_vertices);
+  return independent_set_size;
+}
+
 // Find the connected component for a vertex implemented using the
 // find and update operation for disjoint-set. Recursively traverse
 // the disjoint set structure till you reach a vertex whose connected
diff --git a/internal/ceres/graph_algorithms_test.cc b/internal/ceres/graph_algorithms_test.cc
index 78a0452..7c24476 100644
--- a/internal/ceres/graph_algorithms_test.cc
+++ b/internal/ceres/graph_algorithms_test.cc
@@ -165,7 +165,7 @@
   }
 }
 
-TEST(VertexDegreeLessThan, TotalOrdering) {
+TEST(VertexTotalOrdering, TotalOrdering) {
   Graph<int> graph;
   graph.AddVertex(0);
   graph.AddVertex(1);
@@ -178,7 +178,7 @@
   // 0,1 and 2 have degree 1 and 3 has degree 2.
   graph.AddEdge(0, 1, 1.0);
   graph.AddEdge(2, 3, 1.0);
-  VertexDegreeLessThan<int> less_than(graph);
+  VertexTotalOrdering<int> less_than(graph);
 
   for (int i = 0; i < 4; ++i) {
     EXPECT_FALSE(less_than(i, i)) << "Failing vertex: " << i;
@@ -196,5 +196,49 @@
   }
 }
 
+
+TEST(StableIndependentSet, BreakTies) {
+  Graph<int> graph;
+  graph.AddVertex(0);
+  graph.AddVertex(1);
+  graph.AddVertex(2);
+  graph.AddVertex(3);
+
+  graph.AddEdge(0, 1);
+  graph.AddEdge(0, 2);
+  graph.AddEdge(0, 3);
+  graph.AddEdge(1, 2);
+  graph.AddEdge(1, 3);
+  graph.AddEdge(2, 3);
+
+  // Since this is a completely connected graph, the independent set
+  // contains exactly one vertex. StableIndependentSetOrdering
+  // guarantees that it will always be the first vertex in the
+  // ordering vector.
+  {
+    vector<int> ordering;
+    ordering.push_back(0);
+    ordering.push_back(1);
+    ordering.push_back(2);
+    ordering.push_back(3);
+    const int independent_set_size =
+        StableIndependentSetOrdering(graph, &ordering);
+    EXPECT_EQ(independent_set_size, 1);
+    EXPECT_EQ(ordering[0], 0);
+  }
+
+  {
+    vector<int> ordering;
+    ordering.push_back(1);
+    ordering.push_back(0);
+    ordering.push_back(2);
+    ordering.push_back(3);
+    const int independent_set_size =
+        StableIndependentSetOrdering(graph, &ordering);
+    EXPECT_EQ(independent_set_size, 1);
+    EXPECT_EQ(ordering[0], 1);
+  }
+
+}
 }  // namespace internal
 }  // namespace ceres
diff --git a/internal/ceres/parameter_block_ordering.cc b/internal/ceres/parameter_block_ordering.cc
index e8f626f..190715b 100644
--- a/internal/ceres/parameter_block_ordering.cc
+++ b/internal/ceres/parameter_block_ordering.cc
@@ -42,6 +42,32 @@
 namespace ceres {
 namespace internal {
 
+int ComputeStableSchurOrdering(const Program& program,
+                         vector<ParameterBlock*>* ordering) {
+  CHECK_NOTNULL(ordering)->clear();
+
+  scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program));
+  const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
+  const HashSet<ParameterBlock*>& vertices = graph->vertices();
+  for (int i = 0; i < parameter_blocks.size(); ++i) {
+    if (vertices.count(parameter_blocks[i]) > 0) {
+      ordering->push_back(parameter_blocks[i]);
+    }
+  }
+
+  int independent_set_size = StableIndependentSetOrdering(*graph, ordering);
+
+  // Add the excluded blocks to back of the ordering vector.
+  for (int i = 0; i < parameter_blocks.size(); ++i) {
+    ParameterBlock* parameter_block = parameter_blocks[i];
+    if (parameter_block->IsConstant()) {
+      ordering->push_back(parameter_block);
+    }
+  }
+
+  return independent_set_size;
+}
+
 int ComputeSchurOrdering(const Program& program,
                          vector<ParameterBlock*>* ordering) {
   CHECK_NOTNULL(ordering)->clear();
diff --git a/internal/ceres/parameter_block_ordering.h b/internal/ceres/parameter_block_ordering.h
index a5277a4..4675cb8 100644
--- a/internal/ceres/parameter_block_ordering.h
+++ b/internal/ceres/parameter_block_ordering.h
@@ -58,6 +58,12 @@
 int ComputeSchurOrdering(const Program& program,
                          vector<ParameterBlock* >* ordering);
 
+// Same as above, except that ties while computing the independent set
+// ordering are resolved in favour of the order in which the parameter
+// blocks occur in the program.
+int ComputeStableSchurOrdering(const Program& program,
+                               vector<ParameterBlock* >* ordering);
+
 // Use an approximate independent set ordering to decompose the
 // parameter blocks of a problem in a sequence of independent
 // sets. The ordering covers all the non-constant parameter blocks in
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index ddcbb37..56993c8 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -1451,8 +1451,8 @@
     // 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);
+    const int num_eliminate_blocks = ComputeStableSchurOrdering(*program,
+                                                                &schur_ordering);
 
     CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
         << "Congratulations, you found a Ceres bug! Please report this error "