internal/ceres/graph.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #ifndef CERES_INTERNAL_GRAPH_H_
  32 #define CERES_INTERNAL_GRAPH_H_
  33
  34 #include <limits>
  35 #include <utility>
  36 #include "ceres/integral_types.h"
  37 #include "ceres/map_util.h"
  38 #include "ceres/collections_port.h"
  39 #include "ceres/internal/macros.h"
  40 #include "ceres/types.h"
  41 #include "glog/logging.h"
  42
  43 namespace ceres {
  44 namespace internal {
  45
  46 // A unweighted undirected graph templated over the vertex ids. Vertex
  47 // should be hashable.
  48 template <typename Vertex>
  49 class Graph {
  50  public:
  51   Graph() {}
  52
  53   // Add a vertex.
  54   void AddVertex(const Vertex& vertex) {
  55     if (vertices_.insert(vertex).second) {
  56       edges_[vertex] = HashSet<Vertex>();
  57     }
  58   }
  59
  60   bool RemoveVertex(const Vertex& vertex) {
  61     if (vertices_.find(vertex) == vertices_.end()) {
  62       return false;
  63     }
  64
  65     vertices_.erase(vertex);
  66     const HashSet<Vertex>& sinks = edges_[vertex];
  67     for (typename HashSet<Vertex>::const_iterator it = sinks.begin();
  68          it != sinks.end(); ++it) {
  69       edges_[*it].erase(vertex);
  70     }
  71
  72     edges_.erase(vertex);
  73     return true;
  74   }
  75
  76   // Add an edge between the vertex1 and vertex2. Calling AddEdge on a
  77   // pair of vertices which do not exist in the graph yet will result
  78   // in undefined behavior.
  79   //
  80   // It is legal to call this method repeatedly for the same set of
  81   // vertices.
  82   void AddEdge(const Vertex& vertex1, const Vertex& vertex2) {
  83     DCHECK(vertices_.find(vertex1) != vertices_.end());
  84     DCHECK(vertices_.find(vertex2) != vertices_.end());
  85
  86     if (edges_[vertex1].insert(vertex2).second) {
  87       edges_[vertex2].insert(vertex1);
  88     }
  89   }
  90
  91   // Calling Neighbors on a vertex not in the graph will result in
  92   // undefined behaviour.
  93   const HashSet<Vertex>& Neighbors(const Vertex& vertex) const {
  94     return FindOrDie(edges_, vertex);
  95   }
  96
  97   const HashSet<Vertex>& vertices() const {
  98     return vertices_;
  99   }
 100
 101  private:
 102   HashSet<Vertex> vertices_;
 103   HashMap<Vertex, HashSet<Vertex> > edges_;
 104
 105   CERES_DISALLOW_COPY_AND_ASSIGN(Graph);
 106 };
 107
 108 // A weighted undirected graph templated over the vertex ids. Vertex
 109 // should be hashable and comparable.
 110 template <typename Vertex>
 111 class WeightedGraph {
 112  public:
 113   WeightedGraph() {}
 114
 115   // Add a weighted vertex. If the vertex already exists in the graph,
 116   // its weight is set to the new weight.
 117   void AddVertex(const Vertex& vertex, double weight) {
 118     if (vertices_.find(vertex) == vertices_.end()) {
 119       vertices_.insert(vertex);
 120       edges_[vertex] = HashSet<Vertex>();
 121     }
 122     vertex_weights_[vertex] = weight;
 123   }
 124
 125   // Uses weight = 1.0. If vertex already exists, its weight is set to
 126   // 1.0.
 127   void AddVertex(const Vertex& vertex) {
 128     AddVertex(vertex, 1.0);
 129   }
 130
 131   bool RemoveVertex(const Vertex& vertex) {
 132     if (vertices_.find(vertex) == vertices_.end()) {
 133       return false;
 134     }
 135
 136     vertices_.erase(vertex);
 137     vertex_weights_.erase(vertex);
 138     const HashSet<Vertex>& sinks = edges_[vertex];
 139     for (typename HashSet<Vertex>::const_iterator it = sinks.begin();
 140          it != sinks.end(); ++it) {
 141       if (vertex < *it) {
 142         edge_weights_.erase(std::make_pair(vertex, *it));
 143       } else {
 144         edge_weights_.erase(std::make_pair(*it, vertex));
 145       }
 146       edges_[*it].erase(vertex);
 147     }
 148
 149     edges_.erase(vertex);
 150     return true;
 151   }
 152
 153   // Add a weighted edge between the vertex1 and vertex2. Calling
 154   // AddEdge on a pair of vertices which do not exist in the graph yet
 155   // will result in undefined behavior.
 156   //
 157   // It is legal to call this method repeatedly for the same set of
 158   // vertices.
 159   void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) {
 160     DCHECK(vertices_.find(vertex1) != vertices_.end());
 161     DCHECK(vertices_.find(vertex2) != vertices_.end());
 162
 163     if (edges_[vertex1].insert(vertex2).second) {
 164       edges_[vertex2].insert(vertex1);
 165     }
 166
 167     if (vertex1 < vertex2) {
 168       edge_weights_[std::make_pair(vertex1, vertex2)] = weight;
 169     } else {
 170       edge_weights_[std::make_pair(vertex2, vertex1)] = weight;
 171     }
 172   }
 173
 174   // Uses weight = 1.0.
 175   void AddEdge(const Vertex& vertex1, const Vertex& vertex2) {
 176     AddEdge(vertex1, vertex2, 1.0);
 177   }
 178
 179   // Calling VertexWeight on a vertex not in the graph will result in
 180   // undefined behavior.
 181   double VertexWeight(const Vertex& vertex) const {
 182     return FindOrDie(vertex_weights_, vertex);
 183   }
 184
 185   // Calling EdgeWeight on a pair of vertices where either one of the
 186   // vertices is not present in the graph will result in undefined
 187   // behaviour. If there is no edge connecting vertex1 and vertex2,
 188   // the edge weight is zero.
 189   double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const {
 190     if (vertex1 < vertex2) {
 191       return FindWithDefault(edge_weights_,
 192                              std::make_pair(vertex1, vertex2), 0.0);
 193     } else {
 194       return FindWithDefault(edge_weights_,
 195                              std::make_pair(vertex2, vertex1), 0.0);
 196     }
 197   }
 198
 199   // Calling Neighbors on a vertex not in the graph will result in
 200   // undefined behaviour.
 201   const HashSet<Vertex>& Neighbors(const Vertex& vertex) const {
 202     return FindOrDie(edges_, vertex);
 203   }
 204
 205   const HashSet<Vertex>& vertices() const {
 206     return vertices_;
 207   }
 208
 209   static double InvalidWeight() {
 210     return std::numeric_limits<double>::quiet_NaN();
 211   }
 212
 213  private:
 214   HashSet<Vertex> vertices_;
 215   HashMap<Vertex, double> vertex_weights_;
 216   HashMap<Vertex, HashSet<Vertex> > edges_;
 217   HashMap<std::pair<Vertex, Vertex>, double> edge_weights_;
 218
 219   CERES_DISALLOW_COPY_AND_ASSIGN(WeightedGraph);
 220 };
 221
 222 }  // namespace internal
 223 }  // namespace ceres
 224
 225 #endif  // CERES_INTERNAL_GRAPH_H_