1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
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
43namespace ceres {
44namespace internal {
45
46// A weighted undirected graph templated over the vertex ids. Vertex
47// should be hashable and comparable.
48template <typename Vertex>
49class Graph {
50 public:
51  Graph() {}
52
53  // Add a weighted vertex. If the vertex already exists in the graph,
54  // its weight is set to the new weight.
55  void AddVertex(const Vertex& vertex, double weight) {
56    if (vertices_.find(vertex) == vertices_.end()) {
57      vertices_.insert(vertex);
58      edges_[vertex] = HashSet<Vertex>();
59    }
60    vertex_weights_[vertex] = weight;
61  }
62
63  // Uses weight = 1.0. If vertex already exists, its weight is set to
64  // 1.0.
65  void AddVertex(const Vertex& vertex) {
66    AddVertex(vertex, 1.0);
67  }
68
69  bool RemoveVertex(const Vertex& vertex) {
70    if (vertices_.find(vertex) == vertices_.end()) {
71      return false;
72    }
73
74    vertices_.erase(vertex);
75    vertex_weights_.erase(vertex);
76    const HashSet<Vertex>& sinks = edges_[vertex];
77    for (typename HashSet<Vertex>::const_iterator it = sinks.begin();
78         it != sinks.end(); ++it) {
79      if (vertex < *it) {
80        edge_weights_.erase(make_pair(vertex, *it));
81      } else {
82        edge_weights_.erase(make_pair(*it, vertex));
83      }
84      edges_[*it].erase(vertex);
85    }
86
87    edges_.erase(vertex);
88    return true;
89  }
90
91  // Add a weighted edge between the vertex1 and vertex2. Calling
92  // AddEdge on a pair of vertices which do not exist in the graph yet
93  // will result in undefined behavior.
94  //
95  // It is legal to call this method repeatedly for the same set of
96  // vertices.
97  void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) {
98    DCHECK(vertices_.find(vertex1) != vertices_.end());
99    DCHECK(vertices_.find(vertex2) != vertices_.end());
100
101    if (edges_[vertex1].insert(vertex2).second) {
102      edges_[vertex2].insert(vertex1);
103    }
104
105    if (vertex1 < vertex2) {
106      edge_weights_[make_pair(vertex1, vertex2)] = weight;
107    } else {
108      edge_weights_[make_pair(vertex2, vertex1)] = weight;
109    }
110  }
111
112  // Uses weight = 1.0.
113  void AddEdge(const Vertex& vertex1, const Vertex& vertex2) {
114    AddEdge(vertex1, vertex2, 1.0);
115  }
116
117  // Calling VertexWeight on a vertex not in the graph will result in
118  // undefined behavior.
119  double VertexWeight(const Vertex& vertex) const {
120    return FindOrDie(vertex_weights_, vertex);
121  }
122
123  // Calling EdgeWeight on a pair of vertices where either one of the
124  // vertices is not present in the graph will result in undefined
125  // behaviour. If there is no edge connecting vertex1 and vertex2,
126  // the edge weight is zero.
127  double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const {
128    if (vertex1 < vertex2) {
129      return FindWithDefault(edge_weights_, make_pair(vertex1, vertex2), 0.0);
130    } else {
131      return FindWithDefault(edge_weights_, make_pair(vertex2, vertex1), 0.0);
132    }
133  }
134
135  // Calling Neighbors on a vertex not in the graph will result in
136  // undefined behaviour.
137  const HashSet<Vertex>& Neighbors(const Vertex& vertex) const {
138    return FindOrDie(edges_, vertex);
139  }
140
141  const HashSet<Vertex>& vertices() const {
142    return vertices_;
143  }
144
145  static double InvalidWeight() {
146    return std::numeric_limits<double>::quiet_NaN();
147  };
148
149 private:
150  HashSet<Vertex> vertices_;
151  HashMap<Vertex, double> vertex_weights_;
152  HashMap<Vertex, HashSet<Vertex> > edges_;
153  HashMap<pair<Vertex, Vertex>, double> edge_weights_;
154
155  CERES_DISALLOW_COPY_AND_ASSIGN(Graph);
156};
157
158}  // namespace internal
159}  // namespace ceres
160
161#endif  // CERES_INTERNAL_GRAPH_H_
162