Loading...
Searching...
No Matches
GraphAlgorithms.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2017 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_
18#define IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_
19
20#include <functional>
21#include <list>
22#include <map>
23#include <queue>
24#include <stack>
25#include <utility>
26#include <vector>
27
28#include <ignition/math/config.hh>
31
32namespace ignition
33{
34namespace math
35{
36inline namespace IGNITION_MATH_VERSION_NAMESPACE
37{
38namespace graph
39{
43 using CostInfo = std::pair<double, VertexId>;
44
51 template<typename V, typename E, typename EdgeType>
52 std::vector<VertexId> BreadthFirstSort(const Graph<V, E, EdgeType> &_graph,
53 const VertexId &_from)
54 {
55 // Create an auxiliary graph, where the data is just a boolean value that
56 // stores whether the vertex has been visited or not.
57 Graph<bool, E, EdgeType> visitorGraph;
58
59 // Copy the vertices (just the Id).
60 for (auto const &v : _graph.Vertices())
61 visitorGraph.AddVertex("", false, v.first);
62
63 // Copy the edges (without data).
64 for (auto const &e : _graph.Edges())
65 visitorGraph.AddEdge(e.second.get().Vertices(), E());
66
67 std::vector<VertexId> visited;
68 std::list<VertexId> pending = {_from};
69
70 while (!pending.empty())
71 {
72 auto vId = pending.front();
73 pending.pop_front();
74
75 // If the vertex has been visited, skip.
76 auto &vertex = visitorGraph.VertexFromId(vId);
77 if (vertex.Data())
78 continue;
79
80 visited.push_back(vId);
81 vertex.Data() = true;
82
83 // Add more vertices to visit if they haven't been visited yet.
84 auto adjacents = visitorGraph.AdjacentsFrom(vId);
85 for (auto const &adj : adjacents)
86 {
87 vId = adj.first;
88 auto &v = adj.second.get();
89 if (!v.Data())
90 pending.push_back(vId);
91 }
92 }
93
94 return visited;
95 }
96
103 template<typename V, typename E, typename EdgeType>
104 std::vector<VertexId> DepthFirstSort(const Graph<V, E, EdgeType> &_graph,
105 const VertexId &_from)
106 {
107 // Create an auxiliary graph, where the data is just a boolean value that
108 // stores whether the vertex has been visited or not.
109 Graph<bool, E, EdgeType> visitorGraph;
110
111 // Copy the vertices (just the Id).
112 for (auto const &v : _graph.Vertices())
113 visitorGraph.AddVertex("", false, v.first);
114
115 // Copy the edges (without data).
116 for (auto const &e : _graph.Edges())
117 visitorGraph.AddEdge(e.second.get().Vertices(), E());
118
119 std::vector<VertexId> visited;
120 std::stack<VertexId> pending({_from});
121
122 while (!pending.empty())
123 {
124 auto vId = pending.top();
125 pending.pop();
126
127 // If the vertex has been visited, skip.
128 auto &vertex = visitorGraph.VertexFromId(vId);
129 if (vertex.Data())
130 continue;
131
132 visited.push_back(vId);
133 vertex.Data() = true;
134
135 // Add more vertices to visit if they haven't been visited yet.
136 auto adjacents = visitorGraph.AdjacentsFrom(vId);
137 for (auto const &adj : adjacents)
138 {
139 vId = adj.first;
140 auto &v = adj.second.get();
141 if (!v.Data())
142 pending.push(vId);
143 }
144 }
145
146 return visited;
147 }
148
207 template<typename V, typename E, typename EdgeType>
208 std::map<VertexId, CostInfo> Dijkstra(const Graph<V, E, EdgeType> &_graph,
209 const VertexId &_from,
210 const VertexId &_to = kNullId)
211 {
212 auto allVertices = _graph.Vertices();
213
214 // Sanity check: The source vertex should exist.
215 if (allVertices.find(_from) == allVertices.end())
216 {
217 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
218 return {};
219 }
220
221 // Sanity check: The destination vertex should exist (if used).
222 if (_to != kNullId &&
223 allVertices.find(_to) == allVertices.end())
224 {
225 std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
226 return {};
227 }
228
229 // Store vertices that are being preprocessed.
230 std::priority_queue<CostInfo,
231 std::vector<CostInfo>, std::greater<CostInfo>> pq;
232
233 // Create a map for distances and next neightbor and initialize all
234 // distances as infinite.
235 std::map<VertexId, CostInfo> dist;
236 for (auto const &v : allVertices)
237 {
238 auto id = v.first;
239 dist[id] = std::make_pair(MAX_D, kNullId);
240 }
241
242 // Insert _from in the priority queue and initialize its distance as 0.
243 pq.push(std::make_pair(0.0, _from));
244 dist[_from] = std::make_pair(0.0, _from);
245
246 while (!pq.empty())
247 {
248 // This is the minimum distance vertex.
249 VertexId u = pq.top().second;
250
251 // Shortcut: Destination vertex found, exiting.
252 if (_to != kNullId && _to == u)
253 break;
254
255 pq.pop();
256
257 for (auto const &edgePair : _graph.IncidentsFrom(u))
258 {
259 const auto &edge = edgePair.second.get();
260 const auto &v = edge.From(u);
261 double weight = edge.Weight();
262
263 // If there is shorted path to v through u.
264 if (dist[v].first > dist[u].first + weight)
265 {
266 // Updating distance of v.
267 dist[v] = std::make_pair(dist[u].first + weight, u);
268 pq.push(std::make_pair(dist[v].first, v));
269 }
270 }
271 }
272
273 return dist;
274 }
275
284 template<typename V, typename E>
285 std::vector<UndirectedGraph<V, E>> ConnectedComponents(
286 const UndirectedGraph<V, E> &_graph)
287 {
288 std::map<VertexId, unsigned int> visited;
289 unsigned int componentCount = 0;
290
291 for (auto const &v : _graph.Vertices())
292 {
293 if (visited.find(v.first) == visited.end())
294 {
295 auto component = BreadthFirstSort(_graph, v.first);
296 for (auto const &vId : component)
297 visited[vId] = componentCount;
298 ++componentCount;
299 }
300 }
301
302 std::vector<UndirectedGraph<V, E>> res(componentCount);
303
304 // Create the vertices.
305 for (auto const &vPair : _graph.Vertices())
306 {
307 const auto &v = vPair.second.get();
308 const auto &componentId = visited[v.Id()];
309 res[componentId].AddVertex(v.Name(), v.Data(), v.Id());
310 }
311
312 // Create the edges.
313 for (auto const &ePair : _graph.Edges())
314 {
315 const auto &e = ePair.second.get();
316 const auto &vertices = e.Vertices();
317 const auto &componentId = visited[vertices.first];
318 res[componentId].AddEdge(vertices, e.Data(), e.Weight());
319 }
320
321 return res;
322 }
323}
324}
325}
326}
327#endif
A generic graph class.
Definition Graph.hh:101
EdgeType & AddEdge(const VertexId_P &_vertices, const E &_data, const double _weight=1.0)
Add a new edge to the graph.
Definition Graph.hh:208
const Vertex< V > & VertexFromId(const VertexId &_id) const
Get a reference to a vertex using its Id.
Definition Graph.hh:607
const VertexRef_M< V > Vertices() const
The collection of all vertices in the graph.
Definition Graph.hh:179
const EdgeRef_M< EdgeType > IncidentsFrom(const VertexId &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition Graph.hh:423
const EdgeRef_M< EdgeType > Edges() const
The collection of all edges in the graph.
Definition Graph.hh:263
Vertex< V > & AddVertex(const std::string &_name, const V &_data, const VertexId &_id=kNullId)
Add a new vertex to the graph.
Definition Graph.hh:135
VertexRef_M< V > AdjacentsFrom(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex.
Definition Graph.hh:289
std::vector< UndirectedGraph< V, E > > ConnectedComponents(const UndirectedGraph< V, E > &_graph)
Calculate the connected components of an undirected graph.
Definition GraphAlgorithms.hh:285
std::vector< VertexId > DepthFirstSort(const Graph< V, E, EdgeType > &_graph, const VertexId &_from)
Depth first sort (DFS).
Definition GraphAlgorithms.hh:104
static const VertexId kNullId
Represents an invalid Id.
Definition Vertex.hh:48
std::map< VertexId, CostInfo > Dijkstra(const Graph< V, E, EdgeType > &_graph, const VertexId &_from, const VertexId &_to=kNullId)
Dijkstra algorithm.
Definition GraphAlgorithms.hh:208
std::pair< double, VertexId > CostInfo
Definition GraphAlgorithms.hh:43
std::vector< VertexId > BreadthFirstSort(const Graph< V, E, EdgeType > &_graph, const VertexId &_from)
Breadth first sort (BFS).
Definition GraphAlgorithms.hh:52
static const double MAX_D
Double maximum value. This value will be similar to 1.79769e+308.
Definition Helpers.hh:246
Definition Angle.hh:40