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#ifndef CERES_INTERNAL_GRAPH_H_ 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_GRAPH_H_ 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <limits> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <utility> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/integral_types.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/map_util.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/macros.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A weighted undirected graph templated over the vertex ids. Vertex 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// should be hashable and comparable. 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename Vertex> 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass Graph { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Graph() {} 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add a weighted vertex. If the vertex already exists in the graph, 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // its weight is set to the new weight. 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void AddVertex(const Vertex& vertex, double weight) { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertices_.find(vertex) == vertices_.end()) { 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertices_.insert(vertex); 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edges_[vertex] = HashSet<Vertex>(); 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_weights_[vertex] = weight; 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Uses weight = 1.0. If vertex already exists, its weight is set to 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1.0. 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void AddVertex(const Vertex& vertex) { 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AddVertex(vertex, 1.0); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool RemoveVertex(const Vertex& vertex) { 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertices_.find(vertex) == vertices_.end()) { 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertices_.erase(vertex); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vertex_weights_.erase(vertex); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& sinks = edges_[vertex]; 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != sinks.end(); ++it) { 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex < *it) { 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edge_weights_.erase(make_pair(vertex, *it)); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edge_weights_.erase(make_pair(*it, vertex)); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edges_[*it].erase(vertex); 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edges_.erase(vertex); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add a weighted edge between the vertex1 and vertex2. Calling 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // AddEdge on a pair of vertices which do not exist in the graph yet 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // will result in undefined behavior. 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // It is legal to call this method repeatedly for the same set of 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vertices. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) { 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK(vertices_.find(vertex1) != vertices_.end()); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK(vertices_.find(vertex2) != vertices_.end()); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (edges_[vertex1].insert(vertex2).second) { 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edges_[vertex2].insert(vertex1); 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex1 < vertex2) { 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edge_weights_[make_pair(vertex1, vertex2)] = weight; 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong edge_weights_[make_pair(vertex2, vertex1)] = weight; 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Uses weight = 1.0. 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AddEdge(vertex1, vertex2, 1.0); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Calling VertexWeight on a vertex not in the graph will result in 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // undefined behavior. 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double VertexWeight(const Vertex& vertex) const { 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return FindOrDie(vertex_weights_, vertex); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Calling EdgeWeight on a pair of vertices where either one of the 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vertices is not present in the graph will result in undefined 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // behaviour. If there is no edge connecting vertex1 and vertex2, 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the edge weight is zero. 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const { 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (vertex1 < vertex2) { 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return FindWithDefault(edge_weights_, make_pair(vertex1, vertex2), 0.0); 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return FindWithDefault(edge_weights_, make_pair(vertex2, vertex1), 0.0); 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Calling Neighbors on a vertex not in the graph will result in 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // undefined behaviour. 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return FindOrDie(edges_, vertex); 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<Vertex>& vertices() const { 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return vertices_; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static double InvalidWeight() { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return std::numeric_limits<double>::quiet_NaN(); 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<Vertex> vertices_; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<Vertex, double> vertex_weights_; 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<Vertex, HashSet<Vertex> > edges_; 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<pair<Vertex, Vertex>, double> edge_weights_; 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CERES_DISALLOW_COPY_AND_ASSIGN(Graph); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_GRAPH_H_ 162