Gazebo Math

API Reference

6.15.1
gz/math/graph/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 GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_
18 #define GZ_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 <gz/math/config.hh>
29 #include "gz/math/graph/Graph.hh"
30 #include "gz/math/Helpers.hh"
31 
32 namespace ignition
33 {
34 namespace math
35 {
36 // Inline bracket to help doxygen filtering.
37 inline namespace IGNITION_MATH_VERSION_NAMESPACE {
38 namespace graph
39 {
44 
51  template<typename V, typename E, typename EdgeType>
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>
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 
212  template<typename V, typename E, typename EdgeType>
214  const VertexId &_from,
215  const VertexId &_to = kNullId)
216  {
217  auto allVertices = _graph.Vertices();
218 
219  // Sanity check: The source vertex should exist.
220  if (allVertices.find(_from) == allVertices.end())
221  {
222  std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
223  return {};
224  }
225 
226  // Sanity check: The destination vertex should exist (if used).
227  if (_to != kNullId &&
228  allVertices.find(_to) == allVertices.end())
229  {
230  std::cerr << "Vertex [" << _from << "] Not found" << std::endl;
231  return {};
232  }
233 
234  // Store vertices that are being preprocessed.
237 
238  // Create a map for distances and next neightbor and initialize all
239  // distances as infinite.
241  for (auto const &v : allVertices)
242  {
243  auto id = v.first;
244  dist[id] = std::make_pair(MAX_D, kNullId);
245  }
246 
247  // Insert _from in the priority queue and initialize its distance as 0.
248  pq.push(std::make_pair(0.0, _from));
249  dist[_from] = std::make_pair(0.0, _from);
250 
251  while (!pq.empty())
252  {
253  // This is the minimum distance vertex.
254  VertexId u = pq.top().second;
255 
256  // Shortcut: Destination vertex found, exiting.
257  if (_to != kNullId && _to == u)
258  break;
259 
260  pq.pop();
261 
262  for (auto const &edgePair : _graph.IncidentsFrom(u))
263  {
264  const auto &edge = edgePair.second.get();
265  const auto &v = edge.From(u);
266  double weight = edge.Weight();
267 
268  // If there is shorted path to v through u.
269  if (dist[v].first > dist[u].first + weight)
270  {
271  // Updating distance of v.
272  dist[v] = std::make_pair(dist[u].first + weight, u);
273  pq.push(std::make_pair(dist[v].first, v));
274  }
275  }
276  }
277 
278  return dist;
279  }
280 
289  template<typename V, typename E>
291  const UndirectedGraph<V, E> &_graph)
292  {
294  unsigned int componentCount = 0;
295 
296  for (auto const &v : _graph.Vertices())
297  {
298  if (visited.find(v.first) == visited.end())
299  {
300  auto component = BreadthFirstSort(_graph, v.first);
301  for (auto const &vId : component)
302  visited[vId] = componentCount;
303  ++componentCount;
304  }
305  }
306 
307  std::vector<UndirectedGraph<V, E>> res(componentCount);
308 
309  // Create the vertices.
310  for (auto const &vPair : _graph.Vertices())
311  {
312  const auto &v = vPair.second.get();
313  const auto &componentId = visited[v.Id()];
314  res[componentId].AddVertex(v.Name(), v.Data(), v.Id());
315  }
316 
317  // Create the edges.
318  for (auto const &ePair : _graph.Edges())
319  {
320  const auto &e = ePair.second.get();
321  const auto &vertices = e.Vertices();
322  const auto &componentId = visited[vertices.first];
323  res[componentId].AddEdge(vertices, e.Data(), e.Weight());
324  }
325 
326  return res;
327  }
328 
334  template<typename V, typename E>
336  {
337  std::vector<Vertex<V>> vertices;
339 
340  // Add all vertices.
341  for (auto const &vPair : _graph.Vertices())
342  {
343  vertices.push_back(vPair.second.get());
344  }
345 
346  // Add all edges.
347  for (auto const &ePair : _graph.Edges())
348  {
349  auto const &e = ePair.second.get();
350  edges.push_back({e.Vertices(), e.Data(), e.Weight()});
351  }
352 
353  return UndirectedGraph<V, E>(vertices, edges);
354  }
355 }
356 }
357 }
358 }
359 #endif
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
STL class.
A generic graph class. Both vertices and edges can store user information. A vertex could be created ...
Definition: gz/math/graph/Graph.hh:102
std::vector< VertexId > BreadthFirstSort(const Graph< V, E, EdgeType > &_graph, const VertexId &_from)
Breadth first sort (BFS). Starting from the vertex == _from, it traverses the graph exploring the nei...
Definition: gz/math/graph/GraphAlgorithms.hh:52
T pop_front(T... args)
STL class.
T find(T... args)
std::map< VertexId, CostInfo > Dijkstra(const Graph< V, E, EdgeType > &_graph, const VertexId &_from, const VertexId &_to=kNullId)
Dijkstra algorithm. Find the shortest path between the vertices in a graph. If only a graph and a sou...
Definition: gz/math/graph/GraphAlgorithms.hh:213
STL class.
VertexRef_M< V > AdjacentsFrom(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex. In other words,...
Definition: gz/math/graph/Graph.hh:292
static const VertexId kNullId
Represents an invalid Id.
Definition: gz/math/graph/Vertex.hh:48
T front(T... args)
EdgeType & AddEdge(const VertexId_P &_vertices, const E &_data, const double _weight=1.0)
Add a new edge to the graph.
Definition: gz/math/graph/Graph.hh:211
T push_back(T... args)
const EdgeRef_M< EdgeType > Edges() const
The collection of all edges in the graph.
Definition: gz/math/graph/Graph.hh:266
std::pair< double, VertexId > CostInfo
Used in Dijkstra. For a given source vertex, this pair represents the cost (first element) to reach a...
Definition: gz/math/graph/GraphAlgorithms.hh:43
T top(T... args)
Vertex< V > & AddVertex(const std::string &_name, const V &_data, const VertexId &_id=kNullId)
Add a new vertex to the graph.
Definition: gz/math/graph/Graph.hh:137
std::vector< VertexId > DepthFirstSort(const Graph< V, E, EdgeType > &_graph, const VertexId &_from)
Depth first sort (DFS). Starting from the vertex == _from, it visits the graph as far as possible alo...
Definition: gz/math/graph/GraphAlgorithms.hh:104
std::vector< UndirectedGraph< V, E > > ConnectedComponents(const UndirectedGraph< V, E > &_graph)
Calculate the connected components of an undirected graph. A connected component of an undirected gra...
Definition: gz/math/graph/GraphAlgorithms.hh:290
STL class.
UndirectedGraph< V, E > ToUndirectedGraph(const DirectedGraph< V, E > &_graph)
Copy a DirectedGraph to an UndirectedGraph with the same vertices and edges.
Definition: gz/math/graph/GraphAlgorithms.hh:335
T endl(T... args)
const EdgeRef_M< EdgeType > IncidentsFrom(const VertexId &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition: gz/math/graph/Graph.hh:426
static const double MAX_D
Double maximum value. This value will be similar to 1.79769e+308.
Definition: gz/math/Helpers.hh:257
T empty(T... args)
const Vertex< V > & VertexFromId(const VertexId &_id) const
Get a reference to a vertex using its Id.
Definition: gz/math/graph/Graph.hh:610
T make_pair(T... args)
T end(T... args)
uint64_t VertexId
The unique Id of each vertex.
Definition: gz/math/graph/Vertex.hh:41
const VertexRef_M< V > Vertices() const
The collection of all vertices in the graph.
Definition: gz/math/graph/Graph.hh:181