text: // Ceres Solver - A fast non-linear least squares minimizer
  // Copyright 2015 Google Inc. All rights reserved.
  // http://ceres-solver.org/
  //
  // Redistribution and use in source and binary forms, with or without
  // modification, are permitted provided that the following conditions are met:
  //
  // * Redistributions of source code must retain the above copyright notice,
  //   this list of conditions and the following disclaimer.
  // * Redistributions in binary form must reproduce the above copyright notice,
  //   this list of conditions and the following disclaimer in the documentation
  //   and/or other materials provided with the distribution.
  // * Neither the name of Google Inc. nor the names of its contributors may be
  //   used to endorse or promote products derived from this software without
  //   specific prior written permission.
  //
  // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  // POSSIBILITY OF SUCH DAMAGE.
  //
  // Author: sameeragarwal@google.com (Sameer Agarwal)
  //
  // Various algorithms that operate on undirected graphs.
  
  #ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_
  #define CERES_INTERNAL_GRAPH_ALGORITHMS_H_
  
  #include <algorithm>
  #include <vector>
  #include <utility>
  #include "ceres/collections_port.h"
  #include "ceres/graph.h"
  #include "ceres/wall_time.h"
  #include "glog/logging.h"
  
  namespace ceres {
  namespace internal {
  
  // Compare two vertices of a graph by their degrees, if the degrees
  // are equal then order them by their ids.
  template <typename Vertex>
  class VertexTotalOrdering {
   public:
    explicit VertexTotalOrdering(const Graph<Vertex>& graph)
        : graph_(graph) {}
  
    bool operator()(const Vertex& lhs, const Vertex& rhs) const {
      if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) {
        return lhs < rhs;
      }
      return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size();
    }
  
   private:
    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
  // independent set problem is NP-Hard, but there are effective
  // approximation algorithms available. The implementation here uses a
  // breadth first search that explores the vertices in order of
  // increasing degree. The same idea is used by Saad & Li in "MIQR: A
  // multilevel incomplete QR preconditioner for large sparse
  // least-squares problems", SIMAX, 2007.
  //
  // Given a undirected graph G(V,E), the algorithm is a greedy BFS
  // search where the vertices are explored in increasing order of their
  // degree. The output vector ordering contains elements of S in
  // increasing order of their degree, followed by elements of V - S in
  // increasing order of degree. The return value of the function is the
  // cardinality of S.
  template <typename Vertex>
  int IndependentSetOrdering(const Graph<Vertex>& graph,
                             std::vector<Vertex>* ordering) {
    const HashSet<Vertex>& vertices = graph.vertices();
    const int num_vertices = vertices.size();
  
    CHECK_NOTNULL(ordering);
    ordering->clear();
    ordering->reserve(num_vertices);
  
    // Colors for labeling the graph during the BFS.
    const char kWhite = 0;
    const char kGrey = 1;
    const char kBlack = 2;
  
    // Mark all vertices white.
    HashMap<Vertex, char> vertex_color;
    std::vector<Vertex> vertex_queue;
    for (typename HashSet<Vertex>::const_iterator it = vertices.begin();
         it != vertices.end();
         ++it) {
      vertex_color[*it] = kWhite;
      vertex_queue.push_back(*it);
    }
  
  
    std::sort(vertex_queue.begin(), vertex_queue.end(),
              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.
    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 std::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;
  }
  
  // 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,
                                   std::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;
  
    std::vector<Vertex> vertex_queue(*ordering);
  
    std::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 std::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
  // component has the same id as the vertex itself. Along the way
  // update the connected components of all the vertices. This updating
  // is what gives this data structure its efficiency.
  template <typename Vertex>
  Vertex FindConnectedComponent(const Vertex& vertex,
                                HashMap<Vertex, Vertex>* union_find) {
    typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex);
    DCHECK(it != union_find->end());
    if (it->second != vertex) {
      it->second = FindConnectedComponent(it->second, union_find);
    }
  
    return it->second;
  }
  
  // Compute a degree two constrained Maximum Spanning Tree/forest of
  // the input graph. Caller owns the result.
  //
  // Finding degree 2 spanning tree of a graph is not always
  // possible. For example a star graph, i.e. a graph with n-nodes
  // where one node is connected to the other n-1 nodes does not have
  // a any spanning trees of degree less than n-1.Even if such a tree
  // exists, finding such a tree is NP-Hard.
  
  // We get around both of these problems by using a greedy, degree
  // constrained variant of Kruskal's algorithm. We start with a graph
  // G_T with the same vertex set V as the input graph G(V,E) but an
  // empty edge set. We then iterate over the edges of G in decreasing
  // order of weight, adding them to G_T if doing so does not create a
  // cycle in G_T} and the degree of all the vertices in G_T remains
  // bounded by two. This O(|E|) algorithm results in a degree-2
  // spanning forest, or a collection of linear paths that span the
  // graph G.
  template <typename Vertex>
  WeightedGraph<Vertex>*
  Degree2MaximumSpanningForest(const WeightedGraph<Vertex>& graph) {
    // Array of edges sorted in decreasing order of their weights.
    std::vector<std::pair<double, std::pair<Vertex, Vertex> > > weighted_edges;
    WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>();
  
    // Disjoint-set to keep track of the connected components in the
    // maximum spanning tree.
    HashMap<Vertex, Vertex> disjoint_set;
  
    // Sort of the edges in the graph in decreasing order of their
    // weight. Also add the vertices of the graph to the Maximum
    // Spanning Tree graph and set each vertex to be its own connected
    // component in the disjoint_set structure.
    const HashSet<Vertex>& vertices = graph.vertices();
    for (typename HashSet<Vertex>::const_iterator it = vertices.begin();
         it != vertices.end();
         ++it) {
      const Vertex vertex1 = *it;
      forest->AddVertex(vertex1, graph.VertexWeight(vertex1));
      disjoint_set[vertex1] = vertex1;
  
      const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1);
      for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin();
           it2 != neighbors.end();
           ++it2) {
        const Vertex vertex2 = *it2;
        if (vertex1 >= vertex2) {
          continue;
        }
        const double weight = graph.EdgeWeight(vertex1, vertex2);
        weighted_edges.push_back(
            std::make_pair(weight, std::make_pair(vertex1, vertex2)));
      }
    }
  
    // The elements of this vector, are pairs<edge_weight,
    // edge>. Sorting it using the reverse iterators gives us the edges
    // in decreasing order of edges.
    std::sort(weighted_edges.rbegin(), weighted_edges.rend());
  
    // Greedily add edges to the spanning tree/forest as long as they do
    // not violate the degree/cycle constraint.
    for (int i =0; i < weighted_edges.size(); ++i) {
      const std::pair<Vertex, Vertex>& edge = weighted_edges[i].second;
      const Vertex vertex1 = edge.first;
      const Vertex vertex2 = edge.second;
  
      // Check if either of the vertices are of degree 2 already, in
      // which case adding this edge will violate the degree 2
      // constraint.
      if ((forest->Neighbors(vertex1).size() == 2) ||
          (forest->Neighbors(vertex2).size() == 2)) {
        continue;
      }
  
      // Find the id of the connected component to which the two
      // vertices belong to. If the id is the same, it means that the
      // two of them are already connected to each other via some other
      // vertex, and adding this edge will create a cycle.
      Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set);
      Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set);
  
      if (root1 == root2) {
        continue;
      }
  
      // This edge can be added, add an edge in either direction with
      // the same weight as the original graph.
      const double edge_weight = graph.EdgeWeight(vertex1, vertex2);
      forest->AddEdge(vertex1, vertex2, edge_weight);
      forest->AddEdge(vertex2, vertex1, edge_weight);
  
      // Connected the two connected components by updating the
      // disjoint_set structure. Always connect the connected component
      // with the greater index with the connected component with the
      // smaller index. This should ensure shallower trees, for quicker
      // lookup.
      if (root2 < root1) {
        std::swap(root1, root2);
      }
  
      disjoint_set[root2] = root1;
    }
    return forest;
  }
  
  }  // namespace internal
  }  // namespace ceres
  
  #endif  // CERES_INTERNAL_GRAPH_ALGORITHMS_H_
  