10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Various algorithms that operate on undirected graphs. 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_ 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <glog/logging.h> 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/graph.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compare two vertices of a graph by their degrees. 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename Vertex> 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass VertexDegreeLessThan { 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong explicit VertexDegreeLessThan(const Graph<Vertex>& graph) 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : graph_(graph) {} 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool operator()(const Vertex& lhs, const Vertex& rhs) const { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return lhs < rhs; 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Graph<Vertex>& graph_; 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Order the vertices of a graph using its (approximately) largest 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// independent set, where an independent set of a graph is a set of 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// vertices that have no edges connecting them. The maximum 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// independent set problem is NP-Hard, but there are effective 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// approximation algorithms available. The implementation here uses a 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// breadth first search that explores the vertices in order of 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// increasing degree. The same idea is used by Saad & Li in "MIQR: A 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// multilevel incomplete QR preconditioner for large sparse 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// least-squares problems", SIMAX, 2007. 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Given a undirected graph G(V,E), the algorithm is a greedy BFS 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// search where the vertices are explored in increasing order of their 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// degree. The output vector ordering contains elements of S in 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// increasing order of their degree, followed by elements of V - S in 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// increasing order of degree. The return value of the function is the 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// cardinality of S. 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename Vertex> 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint IndependentSetOrdering(const Graph<Vertex>& graph, 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<Vertex>* ordering) { 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& vertices = graph.vertices(); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_vertices = vertices.size(); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(ordering); 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ordering->clear(); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ordering->reserve(num_vertices); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Colors for labeling the graph during the BFS. 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const char kWhite = 0; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const char kGrey = 1; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const char kBlack = 2; 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Mark all vertices white. 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<Vertex, char> vertex_color; 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<Vertex> vertex_queue; 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != vertices.end(); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_color[*it] = kWhite; 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_queue.push_back(*it); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong sort(vertex_queue.begin(), vertex_queue.end(), 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VertexDegreeLessThan<Vertex>(graph)); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over vertex_queue. Pick the first white vertex, add it 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // to the independent set. Mark it black and its neighbors grey. 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < vertex_queue.size(); ++i) { 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex& vertex = vertex_queue[i]; 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex_color[vertex] != kWhite) { 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ordering->push_back(vertex); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_color[vertex] = kBlack; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != neighbors.end(); 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_color[*it] = kGrey; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int independent_set_size = ordering->size(); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the vertices and add all the grey vertices to the 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // ordering. At this stage there should only be black or grey 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vertices in the graph. 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != vertex_queue.end(); 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex vertex = *it; 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK(vertex_color[vertex] != kWhite); 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex_color[vertex] != kBlack) { 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ordering->push_back(vertex); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(ordering->size(), num_vertices); 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return independent_set_size; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Find the connected component for a vertex implemented using the 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// find and update operation for disjoint-set. Recursively traverse 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the disjoint set structure till you reach a vertex whose connected 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// component has the same id as the vertex itself. Along the way 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// update the connected components of all the vertices. This updating 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// is what gives this data structure its efficiency. 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename Vertex> 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongVertex FindConnectedComponent(const Vertex& vertex, 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<Vertex, Vertex>* union_find) { 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK(it != union_find->end()); 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (it->second != vertex) { 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it->second = FindConnectedComponent(it->second, union_find); 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return it->second; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compute a degree two constrained Maximum Spanning Tree/forest of 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the input graph. Caller owns the result. 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Finding degree 2 spanning tree of a graph is not always 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// possible. For example a star graph, i.e. a graph with n-nodes 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// where one node is connected to the other n-1 nodes does not have 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// a any spanning trees of degree less than n-1.Even if such a tree 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// exists, finding such a tree is NP-Hard. 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// We get around both of these problems by using a greedy, degree 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// constrained variant of Kruskal's algorithm. We start with a graph 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// G_T with the same vertex set V as the input graph G(V,E) but an 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// empty edge set. We then iterate over the edges of G in decreasing 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// order of weight, adding them to G_T if doing so does not create a 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// cycle in G_T} and the degree of all the vertices in G_T remains 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// bounded by two. This O(|E|) algorithm results in a degree-2 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// spanning forest, or a collection of linear paths that span the 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// graph G. 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename Vertex> 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongGraph<Vertex>* 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongDegree2MaximumSpanningForest(const Graph<Vertex>& graph) { 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Array of edges sorted in decreasing order of their weights. 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<pair<double, pair<Vertex, Vertex> > > weighted_edges; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Graph<Vertex>* forest = new Graph<Vertex>(); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Disjoint-set to keep track of the connected components in the 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // maximum spanning tree. 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<Vertex, Vertex> disjoint_set; 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Sort of the edges in the graph in decreasing order of their 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // weight. Also add the vertices of the graph to the Maximum 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Spanning Tree graph and set each vertex to be its own connected 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // component in the disjoint_set structure. 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& vertices = graph.vertices(); 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != vertices.end(); 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex vertex1 = *it; 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong disjoint_set[vertex1] = vertex1; 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it2 != neighbors.end(); 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it2) { 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex vertex2 = *it2; 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex1 >= vertex2) { 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double weight = graph.EdgeWeight(vertex1, vertex2); 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong weighted_edges.push_back(make_pair(weight, make_pair(vertex1, vertex2))); 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The elements of this vector, are pairs<edge_weight, 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // edge>. Sorting it using the reverse iterators gives us the edges 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // in decreasing order of edges. 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong sort(weighted_edges.rbegin(), weighted_edges.rend()); 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Greedily add edges to the spanning tree/forest as long as they do 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // not violate the degree/cycle constraint. 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i =0; i < weighted_edges.size(); ++i) { 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const pair<Vertex, Vertex>& edge = weighted_edges[i].second; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex vertex1 = edge.first; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vertex vertex2 = edge.second; 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check if either of the vertices are of degree 2 already, in 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // which case adding this edge will violate the degree 2 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // constraint. 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((forest->Neighbors(vertex1).size() == 2) || 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (forest->Neighbors(vertex2).size() == 2)) { 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Find the id of the connected component to which the two 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vertices belong to. If the id is the same, it means that the 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // two of them are already connected to each other via some other 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vertex, and adding this edge will create a cycle. 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set); 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set); 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (root1 == root2) { 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This edge can be added, add an edge in either direction with 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the same weight as the original graph. 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double edge_weight = graph.EdgeWeight(vertex1, vertex2); 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong forest->AddEdge(vertex1, vertex2, edge_weight); 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong forest->AddEdge(vertex2, vertex1, edge_weight); 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Connected the two connected components by updating the 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // disjoint_set structure. Always connect the connected component 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // with the greater index with the connected component with the 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // smaller index. This should ensure shallower trees, for quicker 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // lookup. 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (root2 < root1) { 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::swap(root1, root2); 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong disjoint_set[root2] = root1; 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return forest; 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_GRAPH_ALGORITHMS_H_ 271